import serial
import struct
import time
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray

# 定义报文头和数据ID
HEAD = 0xA5
CRC16_POLY = 0x8005
EXPECTED_LENGTH = 61  # 你预计的报文长度

class PublisherNode(Node):

    def __init__(self):
        super().__init__('publisher_node')
        self.publisher_ = self.create_publisher(Float64MultiArray, 'velocity_and_position', 10)
        
    def publish_data(self, velocity, position):
        # 创建消息对象
        msg = Float64MultiArray()
        
        # 设置消息数据，依次为三轴速度和三轴坐标
        msg.data = velocity + position  # 合并速度和坐标，假设长度一致
        
        # 发布消息
        self.publisher_.publish(msg)
        
        # 打印日志
        self.get_logger().info(f"Publishing velocity: {velocity}, position: {position}")

# 计算 CRC16 校验
def calc_crc(data: bytes) -> int:
    crc = 0xFFFF
    for byte in data:
        crc ^= byte
        for _ in range(8):
            if crc & 0x0001:
                crc = (crc >> 1) ^ CRC16_POLY
            else:
                crc >>= 1
    return crc & 0xFFFF

# 解析报文
def parse_message(data: bytes):
    idx = 0

    # 解析报文头
    if data[idx] != HEAD:
        raise ValueError("Invalid header")
    idx += 1

    # 解析数据ID
    data_id = data[idx]
    idx += 1
    print(f"Data ID: {data_id}")

    # 解析数据域长度
    data_len = data[idx]
    idx += 1
    print(f"Data Length: {data_len}")

    # 解析数据域
    data_field = data[idx:idx + data_len]
    idx += data_len
    print(f"Data Field: {data_field.hex()}")

    # 解析 CRC 校验
    crc_received = struct.unpack(">H", data[idx:idx + 2])[0]
    print(f"Received CRC: {crc_received}")

    # 校验 CRC
    # crc_calculated = calc_crc(data[:-2])  # CRC 是不包括最后两位的
    # if crc_received != crc_calculated:
    #     raise ValueError("CRC check failed")

    return data_id, data_field

# 解析输出状态信息
def parse_output_status(data: bytes, node: PublisherNode):
    # 假设报文格式和字段顺序一致
    idx = 0
    # Head
    head = struct.unpack(">H", data[idx:idx+2])[0]
    print(f"frame head: {head}")
    idx += 2

    # 年/月/日
    utc_year = struct.unpack(">H", data[idx:idx+2])[0]
    utc_month = struct.unpack(">B", data[idx+2:idx+3])[0]
    utc_day = struct.unpack(">B", data[idx+3:idx+4])[0]
    idx += 4

    # 时/分/秒/毫秒
    utc_hour = struct.unpack(">B", data[idx:idx+1])[0]
    utc_min = struct.unpack(">B", data[idx+1:idx+2])[0]
    utc_second = struct.unpack(">B", data[idx+2:idx+3])[0]
    utc_mmsec = struct.unpack(">H", data[idx+3:idx+5])[0]
    print(f"UTC Date: {utc_year}-{utc_month}-{utc_day} {utc_hour}:{utc_min}:{utc_second}.{utc_mmsec}")
    idx += 5


    focus_lat = struct.unpack(">i", data[idx:idx+4])[0]
    print(f"focus point latitude: {focus_lat/1e7}")
    idx += 4

    focus_lon = struct.unpack(">i", data[idx:idx+4])[0]
    print(f"focus point lonitude: {focus_lon/1e7}")
    idx += 4

    drone_lat = struct.unpack(">i", data[idx:idx+4])[0]
    print(f"drone latitude: {drone_lat/1e7}")
    idx += 4

    drone_lon = struct.unpack(">i", data[idx:idx+4])[0]
    print(f"drone lonitude: {drone_lon/1e7}")
    idx += 4

    focus_alt = struct.unpack(">i", data[idx:idx+4])[0]
    print(f"focus point altitude: {focus_alt/1e2}m")
    idx += 4

    drone_alt = struct.unpack(">i", data[idx:idx+4])[0]
    print(f"drone altitude: {drone_alt/1e2}m")
    idx += 4

    east_vel = struct.unpack(">i", data[idx:idx+4])[0]
    print(f"east velocity: {east_vel/1e2}m/s")
    idx += 4

    north_vel = struct.unpack(">i", data[idx:idx+4])[0]
    print(f"north velocity: {north_vel/1e2}m/s")
    idx += 4

    up_vel = struct.unpack(">i", data[idx:idx+4])[0]
    print(f"up velocity: {up_vel/1e2}m/s")
    idx += 4

    roll = struct.unpack(">h", data[idx:idx+2])[0]
    print(f"roll: {roll/1e2}")
    idx += 2

    pitch = struct.unpack(">h", data[idx:idx+2])[0]
    print(f"pitch: {pitch/1e2}")
    idx += 2

    yaw = struct.unpack(">h", data[idx:idx+2])[0]
    print(f"yaw: {yaw/1e2}")
    idx += 2

    # publish the topic
    velocity = [east_vel/1e2, north_vel/1e2, up_vel/1e2]
    position = [drone_lat/1e7, drone_lon/1e7, drone_alt/1e7]

    node.publish_data(position, velocity)


# 打开串口读取数据并解析
def read_and_parse_serial_data(port='/dev/ttyACM0', baudrate=115200):
    rclpy.init()
    node = PublisherNode()

    with serial.Serial(port, baudrate, timeout=1) as ser:
        print("Waiting for data...")
        while True:
            # 初始化接收缓冲区
            buffer = bytearray()

            while True:
                # 检查串口是否有数据
                if ser.in_waiting > 0:
                    byte = ser.read(1)

                    # 如果接收到的字节是报文头（0xA5），则开始读取报文
                    if len(buffer) == 0 and byte == bytes([HEAD]):
                        buffer.append(byte[0])  # 添加报文头
                        print(f"Header found, starting to collect data...")

                    elif len(buffer) > 0:
                        buffer.append(byte[0])

                    # 一旦接收到完整的100字节报文，开始解析
                    if len(buffer) == EXPECTED_LENGTH:
                        print(f"Received full message: {buffer.hex()}")
                        try:
                            # 解析报文
                            data_id, data_field = parse_message(buffer)
                    
                            if data_id == 0:
                                print("Parsing Output Status Information...")
                                parse_output_status(data_field, node)
                                
                            else:
                                print(f"Data ID {data_id} requires custom parsing.")
                        except ValueError as e:
                            print(f"Error while parsing data: {e}")
                        
                        # 清空缓冲区，等待下一个报文
                        buffer.clear()

                # time.sleep(0.001)  # 小延时，避免 CPU 占用过高

if __name__ == "__main__":
    
    read_and_parse_serial_data()
